#!/usr/bin/env python
PACKAGE = "my_dwa_planner"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Velocity limits
gen.add("max_vel_x", double_t, 0, "Maximum x velocity (m/s)", 0.5, 0.0, 2.0)
gen.add("min_vel_x", double_t, 0, "Minimum x velocity (m/s)", 0.1, -2.0, 2.0)
gen.add("max_vel_y", double_t, 0, "Maximum y velocity (m/s)", 0.0, 0.0, 2.0)
gen.add("min_vel_y", double_t, 0, "Minimum y velocity (m/s)", 0.0, -2.0, 2.0)
gen.add("max_vel_th", double_t, 0, "Maximum rotational velocity (rad/s)", 1.0, 0.0, 5.0)
gen.add("min_vel_th", double_t, 0, "Minimum rotational velocity (rad/s)", -1.0, -5.0, 0.0)

# Acceleration limits
gen.add("acc_lim_x", double_t, 0, "X acceleration limit (m/s^2)", 2.5, 0.0, 10.0)
gen.add("acc_lim_y", double_t, 0, "Y acceleration limit (m/s^2)", 0.0, 0.0, 10.0)
gen.add("acc_lim_th", double_t, 0, "Rotational acceleration limit (rad/s^2)", 3.2, 0.0, 10.0)

# Algorithm parameters
gen.add("sim_time", double_t, 0, "Simulation time (s)", 1.5, 0.1, 5.0)
gen.add("sim_granularity", double_t, 0, "Simulation step size (m)", 0.025, 0.001, 0.1)
gen.add("angular_sim_granularity", double_t, 0, "Angular step size (rad)", 0.025, 0.001, 0.1)

# Cost function weights
gen.add("pdist_scale", double_t, 0, "Path distance weight", 0.6, 0.0, 2.0)
gen.add("gdist_scale", double_t, 0, "Goal distance weight", 0.8, 0.0, 2.0)
gen.add("occdist_scale", double_t, 0, "Obstacle distance weight", 0.01, 0.0, 1.0)

# Behavior parameters
gen.add("heading_lookahead", double_t, 0, "Heading lookahead distance", 0.325, 0.0, 1.0)
gen.add("oscillation_reset_dist", double_t, 0, "Oscillation reset distance (m)", 0.05, 0.0, 1.0)
gen.add("stop_time_buffer", double_t, 0, "Stop time buffer (s)", 0.2, 0.0, 2.0)

# 在文件末尾添加目标容差参数
gen.add("xy_goal_tolerance", double_t, 0, "XY goal tolerance (m)", 0.1, 0.0, 1.0)
gen.add("yaw_goal_tolerance", double_t, 0, "Yaw goal tolerance (rad)", 0.1, 0.0, 3.14)

exit(gen.generate(PACKAGE, "my_dwa_planner", "DWAPlanner"))